
#include <Arduino.h>
#include <SoftwareSerial.h>
#include "grid_common.h"
#include "serial_communication.h"

SoftwareSerial softSerial4(50,51);

void *ports[] = {&Serial, &Serial3, &softSerial4, &Serial2, &Serial1}; //Used to iterate throught Serial ports

int lastMessageLen = 0;
int lastMessage[MAX_VALUES];

//****************
// Serial Communication Code
//****************

void initCommunication() {
  Serial.begin(9600);
  Serial1.begin(9600);
  Serial2.begin(9600);
  Serial3.begin(9600);
  softSerial4.begin(9600);
}

//Used by Flow to send out state and color to neighboring tiles using MESSAGE TYPE 10
void broadcastStateColor(){
    int values[3] = {10, getMyState(), colorToNumber(getMyColor())};
    //Serial.print("Sending state: "); Serial.print(values[1]);
   // Serial.print(" color: "); Serial.println(values[2]);
    for (int i=1; i<NUMBER_OF_PORTS; i++) {
      portWrite(i, values, 3);
    }
}

boolean isNewMessage(int* values, int len) {
  // If message length is different from last, then it's different, and thus a new message.
  boolean isNewMsg = (len != lastMessageLen);

  // Only when the messdage length is the same, we then compare the actual message values.
  for(int i =0; i<len && !isNewMsg; i++){
      if(values[i] != lastMessage[i]){
        // If any part of message is different from the corresponding part in last message,
        // then it's a new message.
        isNewMsg = true;
      }
  }
  return isNewMsg;
}

void broadcastForward(int* values, int len) {
  // If message length is different from last, then it's different, and thus a new message.
  boolean isNewMsg = isNewMessage(values, len);

  // To avoid exponential messages, we only broadcast new messages.
  if(isNewMsg){
    if (DEBUG) {
      Serial.println("New Message!");Serial.flush();
    }
    for(int i = 3; i < 5; i++) {
      portWrite(i, values, len);
    }

    for(int j =0; j<len; j++){ //Copy values to lastMessage (Note that we leave some garbage values if lastMessage is longer than values)
      lastMessage[j] = values[j]; 
    }
    lastMessageLen = len;
  }else if (DEBUG) {
    Serial.println("Same Message :(");
  }
}

void forward(int* values, int len) {
  // Always forward to TOP.
  portWrite(3, values, len);
  if (getY() == getOriginY()) {
    // If I am on the first row, forward to RIGHT as well.
    portWrite(4, values, len);
  }
}

void portWrite(int portIdx, char ch){
  if (portIdx == SOFT_IDX) {
    ((SoftwareSerial *)ports[portIdx])->print(ch);
  } else {
    ((HardwareSerial *)ports[portIdx])->print(ch);
  }
}

void portWrite(int portIdx, int value){
  if (portIdx == SOFT_IDX) {
    ((SoftwareSerial *)ports[portIdx])->print(value);
  } else {
    ((HardwareSerial *)ports[portIdx])->print(value);
  }
}

void portFlush(int portIdx){
  if (portIdx == SOFT_IDX) {
    ((SoftwareSerial *)ports[portIdx])->flush();
  } else {
    ((HardwareSerial *)ports[portIdx])->flush();
  }
}

void portWrite(int portIdx, int* values, int len) {
  for (int i=0; i<len; i++) {
    portWrite(portIdx, values[i]);
    if (i == (len - 1)) {
      portWrite(portIdx, MESSAGE_SEPERATOR);
    } else {
      portWrite(portIdx, SEPARATOR);
    }
  }
  portFlush(portIdx);
}

char portPeek(int portIdx) {
  char ch;
  if (portIdx == SOFT_IDX) {
    ch = ((SoftwareSerial *)ports[portIdx])->peek();
  } else {
    ch = ((HardwareSerial *)ports[portIdx])->peek();
  }
  return ch;
}

char portForcePeek(int portIdx) {
  char ch = portPeek(portIdx);
  while (ch == -1) {
    //Serial.println("in force peek");
    ch = portPeek(portIdx);
  }
  return ch;
}

char portRead(int portIdx) {
  char ch;
  if (portIdx == SOFT_IDX) {
    ch = ((SoftwareSerial *)ports[portIdx])->read();
  } else {
    ch = ((HardwareSerial *)ports[portIdx])->read();
  }
  return ch;
}

char portForceRead(int portIdx) {
  char ch = portRead(portIdx);
  while (ch == -1) {
    ch = portRead(portIdx);
    //Serial.println("in force read");
  }
  return ch;
}

// it's expected that the Serial port has at least one char available to read.
// integer must be followed by non-digit char. 
int portReadInt(int portIdx) {
  String s;
  char ch = portForcePeek(portIdx);
  while(isDigit(ch)) {
    s = s + portForceRead(portIdx);
    ch = portForcePeek(portIdx);
  }
  if (s.length() == 0) {
    return -1;
  } else {
    return s.toInt();
  }
}

// reads a list of int values from port, returns # of int values read.
// input is expected to be int values separated by single non-digit char,
// ended with MESSAGE_SEPARATOR
int portRead(int portIdx, int* values, int maxLen) {
  int idx = 0;
  char ch = portPeek(portIdx);
  while (ch != -1 && !isDigit(ch)) {
    //discard any none digit chars
    ch = portRead(portIdx);
    //Serial.print("Discarded:"); Serial.print(ch); Serial.print("| from "); Serial.println(TILE_NAMES[portIdx]);
    ch = portPeek(portIdx);
    break;
  }
  if (ch != -1) {
    while (idx < maxLen && (isDigit(ch) || ch == SEPARATOR)) {
      values[idx] = portReadInt(portIdx);
      //Serial.print("Value read:"); Serial.print(values[idx]);
      ch = portForceRead(portIdx);
      //Serial.print(" separator:"); Serial.print(ch); 
      idx++;
    }
  }
  //Serial.println();
  return idx;
}

